#ifndef CAROS_UR_ACTION_INTERFACE_H
#define CAROS_UR_ACTION_INTERFACE_H

#include <caros_universalrobot/ForceModeStartAction.h>
#include <caros_universalrobot/ForceModeStopAction.h>
#include <caros_universalrobot/ForceModeUpdateAction.h>
#include <caros_universalrobot/MoveLinAction.h>
#include <caros_universalrobot/MovePtpAction.h>
#include <caros_universalrobot/MovePtpPathAction.h>
#include <caros_universalrobot/MoveServoQAction.h>
#include <caros_universalrobot/MoveServoStopAction.h>
#include <caros_universalrobot/MoveStopAction.h>
#include <caros_universalrobot/MoveVelQAction.h>
#include <caros_universalrobot/MoveVelTAction.h>
#include <caros_universalrobot/MoveVelStopAction.h>
#include <caros_universalrobot/SetIOAction.h>
#include <caros_universalrobot/SetPayloadAction.h>
#include <caros_universalrobot/ZeroFTsensorAction.h>
#include <caros_universalrobot/SendCustomScriptFunctionAction.h>
#include <caros_universalrobot/SendCustomScriptFileAction.h>
#include <caros_control_msgs/RobotState.h>
#include <caros_control_msgs/Status.h>

#include <rw/math/Q.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/math/VelocityScrew6D.hpp>
#include <rw/math/Wrench6D.hpp>
#include <rw/trajectory/Path.hpp>

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>

#include <string>
#include <tuple>

/* Always publish the latest ur device state */
#define UR_STATE_PUBLISHER_QUEUE_SIZE 30
#define UR_ACTION_INTERFACE_SUB_NAMESPACE "ur_action_interface"
#define ACTION_SERVER_NO_AUTOSTART false
#define ACTION_SERVER_AUTOSTART true

namespace caros
{
/**
 * @brief This is the UR Action interface. It defines an interface for communicating with UR robots
 *
 * In ROS the namespace of the node is used and it is important that not two URActionInterface are running in the
 * same namespace.
 */
class URActionInterface
{
 public:
  explicit URActionInterface(ros::NodeHandle nodehandle);

  virtual ~URActionInterface();

  void forceModeStartCB(const caros_universalrobot::ForceModeStartGoalConstPtr &goal);

  void forceModeStopCB(const caros_universalrobot::ForceModeStopGoalConstPtr &goal);

  void forceModeUpdateCB(const caros_universalrobot::ForceModeUpdateGoalConstPtr &goal);

  void moveLinCB(const caros_universalrobot::MoveLinGoalConstPtr &goal);

  void movePtpCB(const caros_universalrobot::MovePtpGoalConstPtr &goal);

  void movePtpPathCB(const caros_universalrobot::MovePtpPathGoalConstPtr &goal);

  void moveServoQCB(const caros_universalrobot::MoveServoQGoalConstPtr &goal);

  void moveServoStopCB(const caros_universalrobot::MoveServoStopGoalConstPtr &goal);

  void moveStopCB(const caros_universalrobot::MoveStopGoalConstPtr &goal);

  void moveVelQCB(const caros_universalrobot::MoveVelQGoalConstPtr &goal);

  void moveVelTCB(const caros_universalrobot::MoveVelTGoalConstPtr &goal);

  void moveVelStopCB(const caros_universalrobot::MoveVelStopGoalConstPtr &goal);

  void setIOCB(const caros_universalrobot::SetIOGoalConstPtr &goal);

  void setPayloadCB(const caros_universalrobot::SetPayloadGoalConstPtr &goal);

  void zeroFTsensorCB(const caros_universalrobot::ZeroFTsensorGoalConstPtr &goal);

  void sendCustomScriptFunctionCB(const caros_universalrobot::SendCustomScriptFunctionGoalConstPtr &goal);

  void sendCustomScriptFileCB(const caros_universalrobot::SendCustomScriptFileGoalConstPtr &goal);

  //! @brief move robot on a linear Cartesian path (in meters)
  virtual bool urMoveLin(const rw::math::Transform3D<> &target, double speed, double acceleration) = 0;

  //! @brief move robot from joint space point to joint space point (in radians)
  virtual bool urMovePtp(const rw::math::Q &target, double speed, double acceleration) = 0;

  /**
   * @brief move to each joint position specified in a path. with joint positions that includes acceleration,
   * speed and blend for each position.
   */
  virtual bool urMovePtpPath(const rw::trajectory::QPath &q_path) = 0;

  //! @brief move robot in a servoing fashion specifying joint velocity targets (in radians/sec)
  virtual bool urMoveVelQ(const rw::math::Q &q_vel, double acceleration, double time) = 0;

  //! @brief move robot in a servoing fashion specifying a velocity screw in tool coordinates (in meters/sec and
  //! radians/sec)
  virtual bool urMoveVelT(const rw::math::VelocityScrew6D<> &t_vel, double acceleration, double time) = 0;

  //! @brief Stop the speeding
  virtual bool urMoveVelStop() = 0;

  /**
   * @brief move robot in a servoing fashion using joint configurations
   * @param[in] target The joint configurations to servo to.
   *
   * @returns a boolean indicating if the serial device accepted the command
   */
  virtual bool urServoQ(const rw::math::Q &target, const float time, const float lookahead_time, const float gain) = 0;

  /**
   * @brief Stop the servos
   */
  virtual bool urServoStop() = 0;


  //! @brief stops robot and running script
  virtual bool urMoveStop() = 0;

  /**
   * @brief Set robot to be controlled in force mode.
   *
   * @general 6D vector = [x, y, z, rx, ry, rz], C = compliant, NC = Non-Compliant
   * @param[in] ref_t_offset Pose vector that defines the force frame relative to the base frame.
   * @param[in] selection 6D vector, setting C in the axis direction (=1) or NC (=0)
   * @param[in] wrench_target The forces/torques the robot will apply to its environment. The robot adjusts its position
   * along/about compliant axis in order to achieve the specified force/torque. Values have no effect for non-compliant
   * axes
   * @param[in] type An integer [1;3] specifying how the robot interprets the force frame. 1: The force frame is
   * transformed in a way such that its y-axis is aligned with a vector pointing from the robot tcp towards the origin
   *of
   * the force frame. 2: The force frame is not transformed. 3: The force frame is transformed in a way such that its
   * x-axis is the projection of the robot tcp velocity vector onto the x-y plane of the force frame.
   * @param[in] limits 6D vector, C: max tcp speed, NV: max deviation from tcp pose
   *
   * @returns a boolean indicating if the serial device accepted the command
   */
  virtual bool urForceModeStart(const rw::math::Transform3D<> &ref_t_offset, const rw::math::Q &selection,
                                const rw::math::Wrench6D<> &wrench_target, int type, const rw::math::Q &limits) = 0;

  /**
   * @brief Update the wrench the robot will apply to its environment.
   * @param[in] wrench_target The forces/torques the robot will apply to its environment. The robot adjusts its position
   * along/about compliant axis in order to achieve the specified force/torque. Values have no effect for non-compliant
   * axes
   *
   * @returns a boolean indicating if the serial device accepted the command
   */
  virtual bool urForceModeUpdate(const rw::math::Wrench6D<> &wrench_target) = 0;

  /**
   * @brief Resets the robot mode from force mode to normal operation.
   *
   * @returns a boolean indicating if the serial device accepted the command
   */
  virtual bool urForceModeStop() = 0;

  /**
   * @brief Sets the payload of the robot.
   *
   * @param[in] mass The mass of the payload in kg.
   * @param[in] com The displacement of the Center of Mass from the robot TCP.
   */
  virtual bool urSetPayload(const double &mass, const rw::math::Vector3D<> &com) = 0;

  /**
   * @brief Sets the specified IO pin of the robot to high or low based on value provided.
   *
   * @param[in] pin The id of the pin to set
   * @param[in] value The value determines if the pin should be high or low (true means the output is high)
   */
  virtual bool urSetIO(const int &pin, const bool &value) = 0;

  /**
   * @brief Send a custom UR script to the controller.
   *
   * @param[in] function_name Specify a name for the custom script function
   * @param[in] script The custom ur script to be sent to the controller specified as a string,
   * each line must be terminated with a newline. The code will automatically be indented with
   * one tab to fit with the function body.
   */
  virtual bool urSendCustomScriptFunction(const std::string &function_name, const std::string &script) = 0;

  /**
   * @brief Send a custom UR script file to the controller.
   *
   * @param[in] file_path The file path to the custom ur script file
   */
  virtual bool urSendCustomScriptFile(const std::string &file_path) = 0;

  /**
   * @brief Zeroes the TCP force/torque measurement from the builtin force/torque sensor by subtracting the current
   * measurement from the subsequent.
   */
  virtual bool urZeroFtSensor() = 0;

 protected:
  //! publish robot state
  void publishState(const caros_control_msgs::RobotState &state);

 private:
  /**
   * @brief private default constructor.
   * This is declared as private to enforce deriving classes to call an available public constructor and enforce that
   * the ROS actions are properly initialised.
   */
  URActionInterface();

 protected:
  ros::NodeHandle nh_;  // NodeHandle instance must be created before the action servers.
  ros::Publisher ur_state_publisher_;
  actionlib::SimpleActionServer<caros_universalrobot::ForceModeStartAction> as_force_mode_start_;
  actionlib::SimpleActionServer<caros_universalrobot::ForceModeStopAction> as_force_mode_stop_;
  actionlib::SimpleActionServer<caros_universalrobot::ForceModeUpdateAction> as_force_mode_update_;
  actionlib::SimpleActionServer<caros_universalrobot::MoveLinAction> as_move_lin_;
  actionlib::SimpleActionServer<caros_universalrobot::MovePtpAction> as_move_ptp_;
  actionlib::SimpleActionServer<caros_universalrobot::MovePtpPathAction> as_move_ptp_path_;
  actionlib::SimpleActionServer<caros_universalrobot::MoveServoQAction> as_move_servo_q_;
  actionlib::SimpleActionServer<caros_universalrobot::MoveServoStopAction> as_move_servo_stop_;
  actionlib::SimpleActionServer<caros_universalrobot::MoveStopAction> as_move_stop_;
  actionlib::SimpleActionServer<caros_universalrobot::MoveVelQAction> as_move_vel_q_;
  actionlib::SimpleActionServer<caros_universalrobot::MoveVelTAction> as_move_vel_t_;
  actionlib::SimpleActionServer<caros_universalrobot::MoveVelStopAction> as_move_vel_stop_;
  actionlib::SimpleActionServer<caros_universalrobot::SetIOAction> as_set_io_;
  actionlib::SimpleActionServer<caros_universalrobot::SetPayloadAction> as_set_payload_;
  actionlib::SimpleActionServer<caros_universalrobot::ZeroFTsensorAction> as_zero_ftsensor_;
  actionlib::SimpleActionServer<caros_universalrobot::SendCustomScriptFunctionAction> as_send_custom_script_function_;
  actionlib::SimpleActionServer<caros_universalrobot::SendCustomScriptFileAction> as_send_custom_script_file_;

  // create messages that are used to publish feedback/result

  caros_universalrobot::ForceModeStartFeedback force_mode_start_feedback_;
  caros_universalrobot::ForceModeStartResult force_mode_start_result_;

  caros_universalrobot::ForceModeStopFeedback force_mode_stop_feedback_;
  caros_universalrobot::ForceModeStopResult force_mode_stop_result_;

  caros_universalrobot::ForceModeUpdateFeedback force_mode_update_feedback_;
  caros_universalrobot::ForceModeUpdateResult force_mode_update_result_;

  caros_universalrobot::MoveLinFeedback move_lin_feedback_;
  caros_universalrobot::MoveLinResult move_lin_result_;

  caros_universalrobot::MovePtpFeedback move_ptp_feedback_;
  caros_universalrobot::MovePtpResult move_ptp_result_;

  caros_universalrobot::MovePtpPathFeedback move_ptp_path_feedback_;
  caros_universalrobot::MovePtpPathResult move_ptp_path_result_;

  caros_universalrobot::MoveServoQFeedback move_servo_q_feedback_;
  caros_universalrobot::MoveServoQResult move_servo_q_result_;

  caros_universalrobot::MoveServoStopFeedback move_servo_stop_feedback_;
  caros_universalrobot::MoveServoStopResult move_servo_stop_result_;

  caros_universalrobot::MoveStopFeedback move_stop_feedback_;
  caros_universalrobot::MoveStopResult move_stop_result_;

  caros_universalrobot::MoveVelQFeedback move_vel_q_feedback_;
  caros_universalrobot::MoveVelQResult move_vel_q_result_;

  caros_universalrobot::MoveVelTFeedback move_vel_t_feedback_;
  caros_universalrobot::MoveVelTResult move_vel_t_result_;

  caros_universalrobot::MoveVelStopFeedback move_vel_stop_feedback_;
  caros_universalrobot::MoveVelStopResult move_vel_stop_result_;

  caros_universalrobot::SetIOFeedback set_io_feedback_;
  caros_universalrobot::SetIOResult set_io_result_;

  caros_universalrobot::SetPayloadFeedback set_payload_feedback_;
  caros_universalrobot::SetPayloadResult set_payload_result_;

  caros_universalrobot::ZeroFTsensorFeedback zero_ftsensor_feedback_;
  caros_universalrobot::ZeroFTsensorResult zero_ftsensor_result_;

  caros_universalrobot::SendCustomScriptFunctionFeedback send_custom_script_function_feedback_;
  caros_universalrobot::SendCustomScriptFunctionResult send_custom_script_function_result_;

  caros_universalrobot::SendCustomScriptFileFeedback send_custom_script_file_feedback_;
  caros_universalrobot::SendCustomScriptFileResult send_custom_script_file_result_;
};
}  // namespace caros

#endif  // CAROS_UR_ACTION_INTERFACE_H
